/*
 * "Copyright (c) 2007 The Regents of the University College Cork
 * All rights reserved.
 *
 * Permission to use, copy, modify, and distribute this software and its
 * documentation for any purpose, without fee, and without written agreement is
 * hereby granted, provided that the above copyright notice, the following
 * two paragraphs and the author appear in all copies of this software.
 *
 * IN NO EVENT SHALL THE UNIVERSITY COLLEGE CORK BE LIABLE TO ANY PARTY FOR
 * DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT
 * OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY 
 * COLLEGE CORK HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * THE UNIVERSITY COLLEGE CORK SPECIFICALLY DISCLAIMS ANY WARRANTIES,
 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
 * AND FITNESS FOR A PARTICULAR PURPOSE.  THE SOFTWARE PROVIDED HEREUNDER IS
 * ON AN "AS IS" BASIS, AND THE UNIVERSITY COLLEGE CORK HAS NO OBLIGATION TO
 * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS."
 */

/**
 * SchedulerPriorityC Implements a priority task queue. Up to five priorities.
 * The Scheduler will only implement queues for the required priorities. If a queue
 * is not being used by any task of that priority, then the queue is not compiled in 
 * to the code.
 *
 * @author Cormac Duffy
 * @date   October 15 2006
 */

#include "hardware.h"
#include "TaskPriority.h"
#include "context_switch.h"

module PrioritySchedulerC {
  provides interface Scheduler;
  uses interface TaskQueueControl[uint8_t id];
  uses interface McuSleep;
  uses interface Leds;
}
implementation
{
  volatile uint16_t *stack_addr;
  volatile uint8_t idex, oldWaiting;
  volatile norace uint8_t tasksWaiting=0;
  volatile norace uint8_t tasksProcessing;
  norace static context_t contexts[NUM_CONTEXTS]; // Our array of system thread data structs

// Variaveis adicionais
  volatile norace int16_t top=-1;
  volatile norace uint32_t curDeadline;

// Funcoes adicionais
  void pushTaskContext(uint8_t id, uint32_t deadline){
           top++;
           contexts[top].id = id;
           contexts[top].deadline = deadline;
   }
   
	int16_t popTaskContext(){
           if (top == -1) return top;
           else {
                   top--;
                   return top+1;
           }
   }

  void preemption_handler()__attribute__ ((C, spontaneous)){
  	bool empty, segue;
  	do{
		segue = FALSE;
		call TaskQueueControl.runNextTask[EDF]();
		empty = call TaskQueueControl.emptyQueue[EDF]();
		if(empty) segue = FALSE;
		else{
			if(contexts[top].id == BASIC) segue = TRUE;
			else if(call TaskQueueControl.getDeadline[EDF]() < contexts[top].deadline) segue = TRUE;
		}
	}while(segue);
  	//passou do laco, entao...
	if(call TaskQueueControl.emptyQueue[EDF]() == FALSE) tasksWaiting &= ~(1<<EDF);
  	tasksProcessing &= ~(1<<EDF);
	curDeadline = contexts[top].deadline;
	//desliga o bit da fila EDF nas duas variaveis
	POP_CONTEXT(popTaskContext());
  }
  
  command void Scheduler.init()__attribute__((noinline))
  { 
  	uint8_t i=0;
  	for(i=0;i<NUM_PRIORITIES;i++){
  		call TaskQueueControl.initialize[i]();
  	}
  }
  
  
  default async command uint32_t TaskQueueControl.getDeadline[uint8_t id](){ return 0; }
  default command void TaskQueueControl.initialize[uint8_t id](){}
 


  void inline handleTask(){
  	   switch(tasksWaiting){
     	case 2: case 3: 
     		if(call TaskQueueControl.runNextTask[EDF]() == FALSE) //Testa se há alguma tarefa EDF esperando
     			tasksWaiting &= ~(1<<EDF);
     		tasksProcessing &= ~(1<<EDF); //Este & desliga o bit EDF
  			
     	break;
     	case 1:
     		if(call TaskQueueControl.runNextTask[BASIC]() == FALSE) //Testa se há alguma tarefa BASIC esperando
     			tasksWaiting &= ~(1<<BASIC);
     		tasksProcessing &= ~(1<<BASIC); //Este & desliga o bit BASIC
     		
     	break;
     }
  }

  command bool Scheduler.runNextTask()__attribute__((noinline)){
	 handleTask();
     return tasksWaiting;
  }
  
  
  void SwitchMe(uint8_t id)__attribute__((noinline)){
  	atomic{
  	oldWaiting = tasksWaiting;
  	tasksWaiting|=(1<<id);

	if(id == BASICCASE) return;
	else{ // id == EDFCASE

	  	if(tasksProcessing >= (1<<EDF)){ //se ja tem tarefa EDF rodando
	  		if(call TaskQueueControl.getDeadline[EDF]() < curDeadline){
				GET_SP(stack_addr);
				stack_addr -=CONTEXT_DIFF;
			
				pushTaskContext(EDF, curDeadline);
				CONTEXT_SWITCH_PREAMBLE(top,top+1);
				PUSH_CONTEXT(top);
				POP_CONTEXT(top+1);
			}
		}

		else if(tasksProcessing < (1<<HIGHER_PREEMPTIVE_INDEX) && tasksProcessing > 0 ){ //tem tarefa basica processando
			GET_SP(stack_addr);
			stack_addr -=CONTEXT_DIFF;
		
			pushTaskContext(BASIC, 0);
			CONTEXT_SWITCH_PREAMBLE(top,top+1);
			PUSH_CONTEXT(top);
			POP_CONTEXT(top+1);
		}

	}// fim else	

	}
  }
  
  async event void TaskQueueControl.queueNotify[uint8_t id]()__attribute__((noinline)){
  	SwitchMe(id);
  }
  
  async event inline void TaskQueueControl.processStarted[uint8_t id](uint32_t deadline){
	tasksProcessing|=(1<<id);
	curDeadline = deadline;
  }
  
  command void Scheduler.taskLoop(){
    	for (;;){
	    	handleTask();
	    	while(!tasksWaiting){
		    	atomic{
		          call McuSleep.sleep();
			}
	    	}
	} 
  }		
  
  default command error_t TaskQueueControl.runNextTask[uint8_t id](){return FAIL;}
  default async command bool TaskQueueControl.emptyQueue[uint8_t](){return TRUE;}

}

